#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using JigLibX.Collision;
using JigLibX.Math;
using JigLibX.Utils;
#endregion

namespace JigLibX.Physics
{

    /// <summary>
    /// Basic rigid body - can be used as is, or a derived class can
    /// over-ride certain behaviours.
    /// A body can only be added to one physics system at a time!
    /// </summary>
    public partial class Body
    {

        private enum Activity
        {
            Active, Inactive
        }

        private bool bodyEnabled;

        // Don't actually own the skin
        private CollisionSkin collSkin;

        // Prevent velocity updates etc
        private bool immovable;

        // the "working" state
        private Transform transform = new Transform();
        private TransformRate transformRate = new TransformRate();
        private TransformRate tranformRateAux = new TransformRate();

        // Flag that gets set whenever our velocity/angular velocity might have been changed.
        // Used by physics to speed up checking for activation
        private bool velChanged;
        private float invMass;

        // Inverse inertia in world frame
        private Matrix worldInvInertia;

        private Transform oldTransform;
        private TransformRate oldTransformRate;

        // the previous state - copied explicitly using CopyCurrentStateToOld.
        private Transform storedTransform;
        private TransformRate storedTransformRate;

        private Matrix invOrientation;
        private float mass;
        private bool origImmovable;
        private bool doShockProcessing;

        private Matrix bodyInertia;     // Inertia in body Space (not necessarily diagonal)
        private Matrix worldInertia;    // inertia tensor in world space
        private Matrix bodyInvInertia;  // inverse inertia in body frame
        private Vector3 force;          // force etc in world frame
        private Vector3 torque;         // torque in world frame

        private Activity activity;      // for deactivation
        private float inactiveTime;     // gow long we've been still

        // last position for when trying the deactivate
        private Vector3 lastPositionForDeactivation;

        // last orientation for when trying to deactivate
        private Matrix lastOrientationForDeactivation;

        // change for detecting position changes during deactivation
        private float sqDeltaPosThreshold;

        // Change (todo - hacky) for detecting orientation changes during 
        // deactivation
        private float sqDeltaOrientThreshold;

        // How long it takes to go from active to frozen when stationary.
        private float deactivationTime;

        // Velocity below which we're considered still
        private float sqVelocityActivityThreshold;

        // Velocity below which we're considered still - in (radians per sec)^2
        private float sqAngVelocityActivityThreshold;

        // The position stored stored when we need to notify other bodies
        private Vector3 storedPositionForActivation;

        // The list of bodies that need to be activated when we move away from
        // our stored position
        private List<Body> bodiesToBeActivatedOnMovement;

        // Whether this body can freeze (assuming physics freezing is enabled)
        private bool allowFreezing;

        // List of constraint that act on this body
        private List<Constraint> constraints = new List<Constraint>();

        // Helper to stop the velocities getting silly.
        private const float VelMax = 100.0f;
        private const float AngVelMax = 50.0f;

        static int idCounter;
        internal int ID;

        public Body()
        {
            this.ID = idCounter++;

            bodiesToBeActivatedOnMovement = new List<Body>();
            bodyEnabled = false;
            collSkin = null;

            this.Mass = 1.0f;
            this.BodyInertia = Matrix.Identity;

            transform = new Transform();
            transform.Position = Vector3.Zero;
            transform.Orientation = Matrix.Identity;

            immovable = false;
            origImmovable = false;
            doShockProcessing = true;

            force = Vector3.Zero;
            torque = Vector3.Zero;

            velChanged = true;

            activity = Activity.Active;
            inactiveTime = 0.0f;
            deactivationTime = 1.0f;
            SetActivityThreshold(0.5f, 30.0f);
            SetDeactivationThreshold(0.1f, 0.2f);
            allowFreezing = true;
            lastPositionForDeactivation = transform.Position;
            lastOrientationForDeactivation = transform.Orientation;

            CopyCurrentStateToOld();
        }

        /// <summary>
        /// Called right at the end of the timestep to notify the
        /// derived classes.
        /// </summary>
        /// <param name="dt"></param>
        public virtual void PostPhysics(float dt)
        {
        }

        /// <summary>
        /// Register with the physics system.
        /// </summary>
        public virtual void EnableBody()
        {
            if (bodyEnabled) return;
            if (PhysicsSystem.CurrentPhysicsSystem == null) return;

            bodyEnabled = true;
            PhysicsSystem.CurrentPhysicsSystem.AddBody(this);
        }

        /// <summary>
        /// Deregiser from the physics system.
        /// </summary>
        public virtual void DisableBody()
        {
            if (!bodyEnabled) return;
            if (PhysicsSystem.CurrentPhysicsSystem == null) return;

            bodyEnabled = false;
            PhysicsSystem.CurrentPhysicsSystem.RemoveBody(this);
        }

        /// <summary>
        /// Are we registered with the physics system?
        /// </summary>
        public virtual bool IsBodyEnabled
        {
            get { return bodyEnabled; }
        }

        /// <summary>
        /// Allowed to return null if this body doesn't engage
        /// in collisions.
        /// </summary>
        public CollisionSkin CollisionSkin
        {
            get { return collSkin; }
            set { collSkin = value; }
        }

        /// This sets the position (sets the vel to 0), but it also tries
        /// to make sure that any frozen bodies resting against this one
        /// get activated if necessary.  Not very efficient. Be a little
        /// careful about when you call it - it will mess with the physics
        /// collision list.  Also, after this call the object will be
        /// active.
        public void MoveTo(Vector3 pos, Matrix orientation)
        {
            if (bodyEnabled && !IsActive)
            {
                PhysicsSystem.CurrentPhysicsSystem.ActivateObject(this);
            }

            Position = pos;
            Orientation = orientation;
            Velocity = Vector3.Zero;
            AngVel = Vector3.Zero;
            CopyCurrentStateToOld();

            if (this.CollisionSkin != null)
                this.CollisionSkin.SetTransform(oldTransform, transform);
        }

        public Transform Transform
        {
            get { return this.transform; }
            set { this.transform = value; }
        }

        public TransformRate TransformRate
        {
            get { return this.transformRate; }
            set { this.transformRate = value; }
        }

        public Transform OldTransform
        {
            get { return oldTransform; }
        }

        public TransformRate OldTransformRate
        {
            get { return oldTransformRate; }
        }

        public Vector3 Position
        {
            get { return this.transform.Position; }
            set { this.transform.Position = value; }
        }

        public Vector3 OldPosition
        {
            get { return this.oldTransform.Position; }
        }

        public Matrix Orientation
        {
            get { return this.transform.Orientation; }
            set { this.transform.Orientation = value; }
        }

        public Matrix OldOrientation
        {
            get { return this.oldTransform.Orientation; }
        }

        public Vector3 Velocity
        {
            get { return this.transformRate.Velocity; }
            set { this.transformRate.Velocity = value; }
        }

        public Vector3 VelocityAux
        {
            get { return this.tranformRateAux.Velocity; }
            set { this.tranformRateAux.Velocity = value; }
        }

        public Vector3 OldVelocity
        {
            get { return this.oldTransformRate.Velocity; }
        }

        public Vector3 AngVel
        {
            get { return this.transformRate.AngularVelocity; }
            set { this.transformRate.AngularVelocity = value; }
        }

        public Vector3 AngVelAux
        {
            get { return this.tranformRateAux.AngularVelocity; }
            set { this.tranformRateAux.AngularVelocity = value; }
        }

        public Vector3 OldAngVel
        {
            get { return this.oldTransformRate.AngularVelocity; }
        }

        public Vector3 Force
        {
            get { return this.force; }
            set { this.force = value; }
        }

        public Vector3 Torque
        {
            get { return torque; }
            set { this.torque = value; }
        }

        /// <summary>
        /// If you set Mass to zero you are fucked.
        /// </summary>
        public float Mass
        {
            get { return mass; }
            set
            {
                this.mass = value;
                this.invMass = 1.0f / mass;
                SetForceToGravity();
            }
        }

        /// <summary>
        /// If you set InvMass to zero you are fucked.
        /// </summary>
        public float InvMass
        {
            get { return invMass; }
            set
            {
                this.invMass = value;
                this.mass = 1.0f / value;
            }
        }

        /// <summary>
        /// Implementation updates the velocity/angular rotation with the
        /// force/torque.
        /// </summary>
        /// <param name="dt"></param>
        public void UpdateVelocity(float dt)
        {
            if (immovable || !IsActive)
                return;

            transformRate.Velocity += (dt * invMass) * force;
            transformRate.AngularVelocity += Vector3.Transform(torque * dt, worldInvInertia);

            // don't quite get this - calculating angMom from angVel, then applying torque to that, then
            // converting back just results in the simple equation anyway. The extra term just produces
            // weirdness..
            // Vector3 angMom = mWorldInertia * mTransformRate.angVelocity;
            // mTransformRate.angVelocity += mWorldInvInertia * dt * (mTorque - Cross(mTransformRate.angVelocity, mWorldInertia * mTransformRate.angVelocity));

            // TODO implement rotational friction properly
            if (collSkin != null && collSkin.Collisions.Count >= 1)
                transformRate.AngularVelocity *= 0.99f;

        }

        /// <summary>
        /// implementation updates the position/orientation with the
        /// current velocties. 
        /// </summary>
        /// <param name="dt"></param>
        public void UpdatePosition(float dt)
        {
            if (immovable || !IsActive)
                return;

            Vector3 angMomBefore = Vector3.Transform(transformRate.AngularVelocity, worldInertia);
            transform.ApplyTransformRate(transformRate, dt);

            // Matrix Check
            invOrientation = Matrix.Transpose(transform.Orientation);
            // recalculate the world inertia
            worldInvInertia = invOrientation * bodyInvInertia * transform.Orientation;

            worldInertia = invOrientation * bodyInertia * transform.Orientation;
            // conservation of momentum
            transformRate.AngularVelocity = Vector3.Transform(angMomBefore, worldInvInertia);

            if (this.CollisionSkin != null)
                CollisionSkin.SetTransform(oldTransform, transform);
        }

        /// <summary>
        /// Updates the position with the auxilary velocities, and zeros them.
        /// </summary>
        /// <param name="dt"></param>
        public void UpdatePositionWithAux(float dt)
        {
            if (immovable || !IsActive)
            {
                tranformRateAux = TransformRate.Zero;
                return;
            }

            PhysicsSystem physics = PhysicsSystem.CurrentPhysicsSystem;
            int ga = physics.MainGravityAxis;

            // CHECK THIS
            //if (ga != -1)
            //{
            //    mTransformRateAux.velocity[(ga + 1) % 3] *= 0.1f;
            //    mTransformRateAux.velocity[(ga + 2) % 3] *= 0.1f;
            //}

            if (ga != -1)
            {
                int ga2 = (ga + 1) % 3;
                if (ga2 == 0) { tranformRateAux.Velocity.X *= 0.1f; tranformRateAux.Velocity.Y *= 0.1f; }
                else if (ga2 == 1) { tranformRateAux.Velocity.Y *= 0.1f; tranformRateAux.Velocity.Z *= 0.1f; }
                else if (ga2 == 2) { tranformRateAux.Velocity.Z *= 0.1f; tranformRateAux.Velocity.X *= 0.1f; }
            }

            Vector3 angMomBefore;
            Vector3.Transform(ref transformRate.AngularVelocity, ref worldInertia, out angMomBefore);

            transform.ApplyTransformRate(transformRate + tranformRateAux, dt);
            tranformRateAux = TransformRate.Zero;

            invOrientation = Matrix.Transpose(transform.Orientation);
            // recalculate the world inertia
            worldInvInertia =  invOrientation * bodyInvInertia* transform.Orientation;
            worldInertia = invOrientation * bodyInertia* transform.Orientation;

            // conservation of momentum
            transformRate.AngularVelocity = Vector3.Transform(angMomBefore, worldInvInertia);


            if (this.CollisionSkin != null)
                this.CollisionSkin.SetTransform(oldTransform, transform);

        }

        /// <summary>
        /// Used by physics to temporarily make an object immovable -
        /// needs to restore afterwards!
        /// </summary>
        public void InternalSetImmovable()
        {
            origImmovable = immovable;
            immovable = true;
        }

        /// <summary>
        /// Used by physics to temporarily make an object immovable -
        /// needs to restore afterwars!
        /// </summary>
        public void InternalRestoreImmovable()
        {
            immovable = origImmovable;
        }

        public bool VelChanged
        {
            get { return velChanged; }
        }

        public void ClearVelChanged()
        {
            velChanged = false;
        }

        public Matrix BodyInertia
        {
            get { return bodyInertia; }
            set
            {
                this.bodyInertia = value;
                this.bodyInvInertia = Matrix.Invert(value);
            }
        }

        public Matrix BodyInvInertia
        {
            get { return bodyInvInertia; }
            set
            {
                this.bodyInvInertia = value;
                this.bodyInertia = Matrix.Invert(value);
            }
        }

        public Matrix WorldInertia
        {
            get { return worldInertia; }
        }

        public Matrix WorldInvInertia
        {
            get { return worldInvInertia; }
        }

        /// <summary>
        /// Ensures that this object never moves, and reduces collision
        /// checking.
        /// </summary>
        public bool Immovable
        {
            get { return immovable; }
            set
            {
                immovable = value;
                origImmovable = immovable;
                invMass = 0.0f;
                bodyInvInertia = new Matrix();
            }
        }

        public bool IsActive
        {
            get { return (activity == Activity.Active); }
        }

        /// <summary>
        /// Allow the activity to be explicitly set - be careful about
        /// explicitly freezing an object (may become unfrozen anyway).
        /// If setting to ACTIVE, then the activity factor indicates how
        /// active the object is considered to be - a value of 1 means
        /// completely active - a value nearer 0 means that freezing will
        /// happen much sooner (assuming no further movement).
        /// </summary>
        public void SetActive()
        {
            SetActive(1.0f);
        }

        bool recursing = false;
        /// <summary>
        /// Allow the activity to be explicitly set - be careful about
        /// explicitly freezing an object (may become unfrozen anyway).
        /// If setting to ACTIVE, then the activity factor indicates how
        /// active the object is considered to be - a value of 1 means
        /// completely active - a value nearer 0 means that freezing will
        /// happen much sooner (assuming no further movement).
        /// </summary>
        /// <param name="actitityFactor"></param>
        public void SetActive(float actitityFactor)
        {
            PhysicsSystem physics = PhysicsSystem.CurrentPhysicsSystem;

            if ((physics != null) && (!recursing))
            {
                recursing = true;
                physics.ActivateObject(this);
                recursing = false;
            }

            activity = Activity.Active;
            inactiveTime = (1.0f - actitityFactor) * deactivationTime;
        }

        public void SetInactive()
        {
            if (allowFreezing && PhysicsSystem.CurrentPhysicsSystem.IsFreezingEnabled)
                activity = Activity.Inactive;
        }

        /// <summary>
        /// Damp movement as the body approaches deactivation
        /// </summary>
        public void DampForDeactivation()
        {
            float frac = inactiveTime / deactivationTime;

            // r = 1 means don't ever damp
            // r = 0.5 means start to damp when half way
            float r = 0.5f;

            if (frac < r) return;

            float scale = 1.0f - ((frac - r) / (1.0f - r));
            scale = MathHelper.Clamp(scale, 0.0f, 1.0f);

            transformRate.Velocity *= scale;
            transformRate.AngularVelocity *= scale;
        }

        /// <summary>
        /// Indicates if the velocity is above the threshold for freezing
        /// </summary>
        /// <returns></returns>
        public bool GetShouldBeActive()
        {
            return ((transformRate.Velocity.LengthSquared() >
                sqVelocityActivityThreshold) ||
                (transformRate.AngularVelocity.LengthSquared() >
                sqAngVelocityActivityThreshold));
        }

        public bool GetShouldBeActiveAux()
        {
            return ((tranformRateAux.Velocity.LengthSquared() >
                sqVelocityActivityThreshold) ||
                (tranformRateAux.AngularVelocity.LengthSquared() >
                sqAngVelocityActivityThreshold));
        }

        /// <summary>
        /// Set how long it takes to deactivate.
        /// </summary>
        /// <param name="seconds"></param>
        public void SetDeactivationTime(float seconds)
        {
            deactivationTime = seconds;
        }

        /// <summary>
        /// Set what the velocity threshold is for activation.
        /// rot is in deg per second.
        /// </summary>
        /// <param name="vel"></param>
        /// <param name="angVel"></param>
        public void SetActivityThreshold(float vel, float angVel)
        {
            sqVelocityActivityThreshold = vel * vel;
            sqAngVelocityActivityThreshold = MathHelper.ToRadians(angVel) * MathHelper.ToRadians(angVel);
        }

        /// <summary>
        /// Set the thresholds for deactivation.
        /// </summary>
        /// <param name="posThreshold"></param>
        /// <param name="orientThreshold"></param>
        public void SetDeactivationThreshold(float posThreshold, float orientThreshold)
        {
            sqDeltaPosThreshold = posThreshold * posThreshold;
            sqDeltaOrientThreshold = orientThreshold * orientThreshold;
        }

        /// <summary>
        /// Allows setting of whether this body ever freezes
        /// </summary>
        public bool AllowFreezing
        {
            get { return allowFreezing; }
            set
            {
                allowFreezing = value;
                if (!value) SetActive();
            }
        }

        /// <summary>
        /// Bodies that use a lot of constraints don't behave very well during the
        /// physics shock step, so they can bypass it.
        /// </summary>
        public bool DoShockProcessing
        {
            get { return doShockProcessing; }
            set { doShockProcessing = value; }
        }

        public void LimitVel()
        {
            transformRate.Velocity.X = MathHelper.Clamp(transformRate.Velocity.X, -VelMax, VelMax);
            transformRate.Velocity.Y = MathHelper.Clamp(transformRate.Velocity.Y, -VelMax, VelMax);
            transformRate.Velocity.Z = MathHelper.Clamp(transformRate.Velocity.Z, -VelMax, VelMax);
        }

        public void LimitAngVel()
        {
            float fX = System.Math.Abs(transformRate.AngularVelocity.X) / AngVelMax;
            float fY = System.Math.Abs(transformRate.AngularVelocity.Y) / AngVelMax;
            float fZ = System.Math.Abs(transformRate.AngularVelocity.Z) / AngVelMax;

            float f = MathHelper.Max(fX, fY);
            f = MathHelper.Max(f, fZ);

            if (f > 1.0f)
                transformRate.AngularVelocity /= f;
        }

        /// <summary>
        /// Returns the velocity of a point at body-relative position
        /// (in world frame) relPos
        /// </summary>
        /// <param name="relPos"></param>
        /// <returns></returns>
        public Vector3 GetVelocity(Vector3 relPos)
        {
            Vector3 result;
            float fX = transformRate.Velocity.X + transformRate.AngularVelocity.Y * relPos.Z - transformRate.AngularVelocity.Z * relPos.Y;
            float fY = transformRate.Velocity.Y + transformRate.AngularVelocity.Z * relPos.X - transformRate.AngularVelocity.X * relPos.Z;
            float fZ = transformRate.Velocity.Z + transformRate.AngularVelocity.X * relPos.Y - transformRate.AngularVelocity.Y * relPos.X;
            result.X = fX;
            result.Y = fY;
            result.Z = fZ;
            return result;
        }


        /// <summary>
        /// Returns the velocity of a point at body-relative position
        /// (in world frame) relPos
        /// </summary>
        /// <param name="relPos"></param>
        /// <param name="result"></param>
        public void GetVelocity(ref Vector3 relPos, out Vector3 result)
        {
            float fX = transformRate.Velocity.X + transformRate.AngularVelocity.Y * relPos.Z - transformRate.AngularVelocity.Z * relPos.Y;
            float fY = transformRate.Velocity.Y + transformRate.AngularVelocity.Z * relPos.X - transformRate.AngularVelocity.X * relPos.Z;
            float fZ = transformRate.Velocity.Z + transformRate.AngularVelocity.X * relPos.Y - transformRate.AngularVelocity.Y * relPos.X;
            result.X = fX;
            result.Y = fY;
            result.Z = fZ;
        }

        /// <summary>
        /// As GetVelocity but just uses the aux velocities
        /// </summary>
        /// <param name="relPos"></param>
        /// <returns></returns>
        public Vector3 GetVelocityAux(Vector3 relPos)
        {
            Vector3 result;
            float fX = tranformRateAux.Velocity.X + tranformRateAux.AngularVelocity.Y * relPos.Z - tranformRateAux.AngularVelocity.Z * relPos.Y;
            float fY = tranformRateAux.Velocity.Y + tranformRateAux.AngularVelocity.Z * relPos.X - tranformRateAux.AngularVelocity.X * relPos.Z;
            float fZ = tranformRateAux.Velocity.Z + tranformRateAux.AngularVelocity.X * relPos.Y - tranformRateAux.AngularVelocity.Y * relPos.X;
            result.X = fX;
            result.Y = fY;
            result.Z = fZ;
            return result;
        }

        /// <summary>
        /// As GetVelocity but just uses the aux velocities
        /// </summary>
        /// <param name="relPos"></param>
        /// <param name="result"></param>
        public void GetVelocityAux(ref Vector3 relPos, out Vector3 result)
        {
            float fX = tranformRateAux.Velocity.X + tranformRateAux.AngularVelocity.Y * relPos.Z - tranformRateAux.AngularVelocity.Z * relPos.Y;
            float fY = tranformRateAux.Velocity.Y + tranformRateAux.AngularVelocity.Z * relPos.X - tranformRateAux.AngularVelocity.X * relPos.Z;
            float fZ = tranformRateAux.Velocity.Z + tranformRateAux.AngularVelocity.X * relPos.Y - tranformRateAux.AngularVelocity.Y * relPos.X;
            result.X = fX;
            result.Y = fY;
            result.Z = fZ;
        }

        /// <summary>
        /// Function provided for use of physics system. Activates any
        /// body in its list if it's moved more than a certain distance,
        /// in which case it also clears its list.
        /// </summary>
        public void DoMovementActivations()
        {
            if (bodiesToBeActivatedOnMovement.Count == 0)
                return;

            if ((transform.Position - storedPositionForActivation).LengthSquared() < sqDeltaPosThreshold)
                return;

            for (int i = 0; i < bodiesToBeActivatedOnMovement.Count; i++)
            {
                PhysicsSystem.CurrentPhysicsSystem.ActivateObject(bodiesToBeActivatedOnMovement[i]);
            }

            bodiesToBeActivatedOnMovement.Clear();
        }

        /// <summary>
        /// adds the other body to the list of bodies to be activated if
        /// this body moves more than a certain distance from either a
        /// previously stored position, or the position passed in.
        /// </summary>
        /// <param name="pos"></param>
        /// <param name="otherBody"></param>
        public void AddMovementActivation(Vector3 pos, Body otherBody)
        {
            if (bodiesToBeActivatedOnMovement.Contains(otherBody))
                return;

            if (bodiesToBeActivatedOnMovement.Count == 0)
                storedPositionForActivation = pos;

            bodiesToBeActivatedOnMovement.Add(otherBody);
        }

        /// <summary>
        /// Function provided for the use of Physics system
        /// </summary>
        /// <param name="dt"></param>
        public void TryToFreeze(float dt)
        {
            if (!allowFreezing || immovable || !IsActive)
                return;

            Vector3 deltaVec;
            Vector3.Subtract(ref transform.Position, ref lastPositionForDeactivation, out deltaVec);

            if (deltaVec.LengthSquared() > sqDeltaPosThreshold)
            {
                lastPositionForDeactivation = transform.Position;
                inactiveTime = 0.0f;
                return;
            }

            // ugly - use quaternions
            Matrix deltaMat = transform.Orientation - lastOrientationForDeactivation;

            if ((deltaMat.Backward.LengthSquared() > sqDeltaOrientThreshold) ||
                (deltaMat.Right.LengthSquared() > sqDeltaOrientThreshold) ||
                (deltaMat.Up.LengthSquared() > sqDeltaOrientThreshold))
            {
                lastOrientationForDeactivation = transform.Orientation;
                inactiveTime = 0.0f;
            }

            if (GetShouldBeActive())
                return;

            inactiveTime += dt;

            if (inactiveTime > deactivationTime)
            {
                lastOrientationForDeactivation = transform.Orientation;
                lastPositionForDeactivation = transform.Position;
                SetInactive();
            }
        }


        public void SetOrientation(Matrix orient)
        {
            transform.Orientation = orient;
            Matrix.Transpose(ref transform.Orientation, out invOrientation);

            worldInvInertia = bodyInvInertia * invOrientation * transform.Orientation;
            worldInertia = bodyInertia * invOrientation * transform.Orientation;
        }

        #region Add impulses in the world coordinate frame
        public void ApplyWorldImpulse(Vector3 impulse)
        {
            if (immovable) return;
            transformRate.Velocity = transformRate.Velocity + invMass * impulse;
            velChanged = true;
        }

        public void ApplyNegativeWorldImpulse(Vector3 impulse)
        {
            ApplyWorldImpulse(-impulse);
        }

        public void ApplyWorldImpulseAux(Vector3 impulse)
        {
            if (immovable) return;
            tranformRateAux.Velocity = tranformRateAux.Velocity + invMass * impulse;
            velChanged = true;
        }

        public void ApplyNegativeWorldImpulseAux(Vector3 impulse)
        {
            ApplyWorldImpulseAux(-impulse);
        }

        public void ApplyWorldImpulse(Vector3 impulse, Vector3 pos)
        {
            ApplyBodyWorldImpulse(impulse, pos - transform.Position);
        }

        public void ApplyNegativeWorldImpulse(Vector3 impulse, Vector3 pos)
        {
            ApplyNegativeBodyWorldImpulse(impulse, pos - transform.Position);
        }

        public void ApplyWorldImpulseAux(Vector3 impulse, Vector3 pos)
        {
            ApplyBodyWorldImpulseAux(impulse, pos - transform.Position);
        }

        public void ApplyNegativeWorldImpulseAux(Vector3 impulse, Vector3 pos)
        {
            ApplyNegativeBodyWorldImpulseAux(impulse, pos - transform.Position);
        }

        public void ApplyWorldAngImpulse(Vector3 angImpulse)
        {
            if (immovable) return;
            transformRate.AngularVelocity += Vector3.Transform(angImpulse, worldInvInertia);
            velChanged = true;
        }

        #endregion

        #region Add impulses at a position offset in world space

        public void ApplyBodyWorldImpulse(Vector3 impulse, Vector3 delta)
        {
            if (immovable) return;
            transformRate.Velocity = transformRate.Velocity + invMass * impulse;
            transformRate.AngularVelocity += Vector3.Transform(Vector3.Cross(delta, impulse), worldInvInertia);
            velChanged = true;
        }

        public void ApplyNegativeBodyWorldImpulse(Vector3 impulse, Vector3 delta)
        {
            ApplyBodyWorldImpulse(-impulse, delta);
        }

        public void ApplyBodyWorldImpulseAux(Vector3 impulse, Vector3 delta)
        {
            if (immovable) return;
            tranformRateAux.Velocity = tranformRateAux.Velocity + invMass * impulse;
            tranformRateAux.AngularVelocity += Vector3.Transform(Vector3.Cross(delta, impulse), worldInvInertia);
            velChanged = true;
        }

        public void ApplyNegativeBodyWorldImpulseAux(Vector3 impulse, Vector3 delta)
        {
            ApplyBodyWorldImpulseAux(-impulse, delta);
        }

        #endregion

        #region Add impulses in the body coordinate frame

        public void ApplyBodyImpulse(Vector3 impulse)
        {
            ApplyWorldImpulse(Vector3.Transform(impulse, transform.Orientation));
        }

        public void ApplyNegativeBodyImpulse(Vector3 impulse)
        {
            ApplyNegativeWorldImpulse(Vector3.Transform(impulse, transform.Orientation));
        }

        public void ApplyBodyImpulse(Vector3 impulse, Vector3 pos)
        {
            ApplyWorldImpulse(Vector3.Transform(impulse, transform.Orientation), transform.Position + Vector3.Transform(pos, transform.Orientation));
        }

        public void ApplyNegativeBodyImpulse(Vector3 impulse, Vector3 pos)
        {
            ApplyNegativeWorldImpulse(Vector3.Transform(impulse, transform.Orientation), transform.Position + Vector3.Transform(pos, transform.Orientation));
        }

        public void ApplyBodyAngImpulse(Vector3 angImpulse)
        {
            ApplyWorldAngImpulse(Vector3.Transform(angImpulse, transform.Orientation));
        }

        #endregion

        #region Add forces in the world coordinate frame

        public void AddWorldForce(Vector3 force)
        {
            if (immovable) return;
            this.force += force;
            this.velChanged = true;
        }

        public void AddWorldForce(Vector3 force, Vector3 pos)
        {
            if (immovable) return;
            this.force += force;
            this.torque += Vector3.Cross(pos - transform.Position, force);
            velChanged = true;
        }

        public void AddWorldTorque(Vector3 torque)
        {
            if (immovable) return;
            this.torque += torque;
            velChanged = true;
        }

        #endregion

        #region Add forces in the body coordinate frame

        public void AddBodyForce(Vector3 force)
        {
            AddWorldForce(Vector3.Transform(force, transform.Orientation));
        }

        public void AddBodyForce(Vector3 force, Vector3 pos)
        {
            AddWorldForce(Vector3.Transform(force, transform.Orientation), transform.Position + Vector3.Transform(pos, transform.Orientation));
        }

        public void AddBodyTorque(Vector3 torque)
        {
            AddWorldTorque(Vector3.Transform(torque, transform.Orientation));
        }

        #endregion

        /// <summary>
        /// This just sets all forces/impulses etc to zero
        /// </summary>
        public void ClearForces()
        {
            force = Vector3.Zero;
            torque = Vector3.Zero;
        }

        /// <summary>
        /// Sets the force on this object to the global gravitational force 
        /// (taken from the physics system)
        /// </summary>
        public void SetForceToGravity()
        {
            if (PhysicsSystem.CurrentPhysicsSystem != null)
                force = mass * PhysicsSystem.CurrentPhysicsSystem.Gravity;
            else
                force = Vector3.Zero;
        }

        /// <summary>
        /// allow the body to add on any additional forces (including
        /// gravity)/impulses etc. Default behaviour sets to gravity.
        /// </summary>
        /// <param name="dt"></param>
        public virtual void AddExternalForces(float dt)
        {
            torque = Vector3.Zero;
            SetForceToGravity();
        }

        /// <summary>
        /// copies the current position etc to old - normally called only
        /// by tPhysicsSystem.
        /// </summary>
        public void CopyCurrentStateToOld()
        {
            oldTransform = transform;
            oldTransformRate = transformRate;
        }

        public void SetConstraintsAndCollisionsUnsatisfied()
        {
            foreach (Constraint constraint in constraints)
                constraint.Satisfied = false;

            if (collSkin == null) return;

            foreach (CollisionInfo info in collSkin.Collisions)
                info.Satisfied = false;
        }

        /// <summary>
        /// Copy our current state (position, velocity etc) into the stored state
        /// </summary>
        public void StoreState()
        {
            storedTransform = transform;
            storedTransformRate = transformRate;
        }

        /// <summary>
        /// Restore from the stored state into our current state.
        /// </summary>
        public void RestoreState()
        {
            transform = storedTransform;
            transformRate = storedTransformRate;

            invOrientation = Matrix.Transpose(transform.Orientation);
            worldInvInertia = invOrientation *bodyInvInertia* transform.Orientation;
            worldInertia = invOrientation *bodyInertia * transform.Orientation;
        }

        public void RemoveConstraint(Constraint constraint)
        {
            //if (this.constraints.Contains(constraint))
            this.constraints.Remove(constraint);
        }

        public void AddConstraint(Constraint constraint)
        {
            if (!this.constraints.Contains(constraint))
                this.constraints.Add(constraint);
        }
    }
}
